#include "GPServerSystem.h"
#include "GPRegisterThreads.h"

#include "raknet/MessageIdentifiers.h"
#include "raknet/RakNetworkFactory.h"
#include "raknet/RakNetStatistics.h"
#include "GPPackets.h"

#include <fstream>
#include <algorithm>

/*!
 * \brief The constructor of the server
 * 
 * \param port
 * The port on which the server will listen to incoming connections
 * 
 * \param framerate
 * The framerate at which the clients will be update.
 * 
 * \param physics
 * The physics system that is controlled by the server
 *
 * This constructor stores the parameters for the server and sets
 *  the last update time to the current time.
 * 
 * \see
 * GPPhysicsSystem
 */
GPServerSystem::GPServerSystem(int port, double framerate, GPPhysicsSystem* physics) : GPThreadedSystem(false), port_(port), framerate_(framerate), physics_(physics) {
	lastupdate_ = getTime();
}

/*!
 * \brief The default destructor 
 */
GPServerSystem::~GPServerSystem() {
}

/*!
 * \brief The initialization function of the server
 * 
 * \returns true if the initialization succeeds
 * 
 * This function initialized the server system. It first creates a peer interface,
 * which it then starts.
 */
bool GPServerSystem::initialize() {
	estimationOrder_= atoi(settings_["prediction"].c_str());
	// There is a limit to the number of connections allowed, currently set to 100
	int maxConnectionsAllowed = 100;
	// The maximum players per server is also set to 100
	int maxPlayersPerServer   = 100;
	
	// The peer interface is created
	peer_ = RakNetworkFactory::GetRakPeerInterface();

	// The peer interface is started with a sleep time for the socket thread of 5 microseconds
	if (!(peer_->Startup(maxConnectionsAllowed, 5, &SocketDescriptor(port_,0), 1)))
		return false;
	
	// Set the maximum allowed incoming connection
	peer_->SetMaximumIncomingConnections(maxPlayersPerServer);

	return true;
}

/*!
 * \brief The actual function that is called when running the thread.
 * 
 * This is the real function of the server system. Here, the packets
 * are received and handled.
 */
void GPServerSystem::run() {
	
	// calculate the time difference between two updates
	double delta_t = 1.0 / framerate_;

	// while the server does not have to stop...
	while (!hasToStop_) {
		// receive a packet
		Packet *packet = peer_->Receive();
		// only when a packet is received handle it
		if (packet) receivePacket(packet);
		// and deallocate the packet.
		peer_->DeallocatePacket(packet);

		// if there are any clients...
		if (clients_.size()>0) {
			// ... and the time since the previous update is large enough...
			walltime now = getTime();
			if (diffWalltime(lastupdate_, now)>delta_t) {

				// make the update thread-safe
				synchronizeEnter();
				physics_->synchronizeEnter();

				// update the clients
				update();

				// set the time since the last update
				lastupdate_ = now;

				// and leave the critical section
				physics_->synchronizeLeave();
				synchronizeLeave();
			}
		}

		// sleep for 10 milliseconds
		microSleep(10000);
	}

	// shutdown the peer interface
	peer_->Shutdown(300);

	// and destroy the object
	RakNetworkFactory::DestroyRakPeerInterface(peer_);
}


/*!
 * \brief When a packet is received, this function is called.
 * 
 * \param packet The packet that is received.
 *
 * When a packet is received, this function handles it. There
 * are several packet types that the server accepts.
 *
 * \see 
 * ID_NEW_INCOMING_CONNECTION | ID_DISCONNECTION_NOTIFICATION | ID_REQ_CREATE_BOX | ID_REQ_CREATE_SPHERE
 */
void GPServerSystem::receivePacket(Packet* packet) {

	int                      objectID = 0;
	const RakNetStatistics*  stats    = NULL;
	unsigned int             bitssend = 0;
	unsigned int             bitsrecv = 0;

	RequestCreateBoxPacket*     rcbp  = NULL;
	RequestCreateSpherePacket*  rcsp  = NULL;

	PredictionData *predictionPacket=new PredictionData;
	// the iterator to iterate along all clients
	std::vector<SystemAddress>::iterator client;

	// switch to the packet type that was received
	switch ((unsigned char)packet->data[0]) {

		// a new incoming connection
		case ID_NEW_INCOMING_CONNECTION:

			// make it thread-safe
			synchronizeEnter();
			
			// store the client
			clients_.push_back(packet->systemAddress);

			// initialize the client with the current state of the world
			initializeClient(packet->systemAddress);

			// store to time it was connected
			connectedTime_[packet->systemAddress] = getTime();

			// show the user that a client connected
			std::cout << "Client connected from " << packet->systemAddress.ToString() << std::endl;

			// leave the critical section
			synchronizeLeave();
			break;

		// a client disconnected
		case ID_DISCONNECTION_NOTIFICATION:

			// make it thread-safe
			synchronizeEnter();

			// get the statistics of the connection
			stats = peer_->GetStatistics(packet->systemAddress);
			bitssend = stats->totalBitsSent;
			bitsrecv = stats->bitsReceived;

			// and show the user
			std::cout << "Bandwidth used (sending, kb/s):   " << ((double)bitssend/diffWalltime(connectedTime_[packet->systemAddress],getTime()))/1024.0 << std::endl;
			std::cout << "Bandwidth used (receiving, kb/s): " << ((double)bitsrecv/diffWalltime(connectedTime_[packet->systemAddress],getTime()))/1024.0 << std::endl;
			
			// remove the client from the list
			clients_.erase( std::find(clients_.begin(), clients_.end(), packet->systemAddress) );

			// and notify the user that a client disconnected
			std::cout << "Client disconnected " << packet->systemAddress.ToString() << std::endl;
			std::cout << "Still " << clients_.size() << " connected." << std::endl;

			// leave the critical section
			synchronizeLeave();
			break;

		// a client requested that a box would be created
		case ID_REQ_CREATE_BOX:

			// make it thread-safe
			synchronizeEnter();

			// get the packet in the correct format
			rcbp = (RequestCreateBoxPacket*)(packet->data);

			// create a box in the physics system and retrieve an object id
			objectID = createBox(rcbp->w, rcbp->h, rcbp->d, rcbp->x, rcbp->y, rcbp->z, rcbp->qx, rcbp->qy, rcbp->qz, rcbp->qw, rcbp->vx, rcbp->vy, rcbp->vz, rcbp->wx, rcbp->wy, rcbp->wz, rcbp->mass, rcbp->dynamic);

			// create a packet to send to the clients
			CreateBoxPacket cbp;
			cbp.w  = rcbp->w;  cbp.h  = rcbp->h;  cbp.d = rcbp->d;
			cbp.x  = rcbp->x;  cbp.y  = rcbp->y;  cbp.z = rcbp->z;
			cbp.qx = rcbp->qx; cbp.qy = rcbp->qy; cbp.qz = rcbp->qz; cbp.qw = rcbp->qw;
			cbp.vx = rcbp->vx; cbp.vy = rcbp->vy; cbp.vz = rcbp->vz; 
			cbp.dynamic=rcbp->dynamic;
			cbp.wx = rcbp->wx; cbp.wy = rcbp->wy; cbp.wz = rcbp->wz;

			cbp.typeID   = ID_CREATE_BOX;
			cbp.objectID = objectID;
			sprintf_s(cbp.material, 10, "vw/red");


			//create a data struct
			
			predictionPacket->lastupdate = getTime();
			predictionPacket->x = rcbp->x;
			predictionPacket->y = rcbp->y;
			predictionPacket->z = rcbp->z;
			predictionPacket->vx = rcbp->vx;
			predictionPacket->vy = rcbp->vy;
			predictionPacket->vz = rcbp->vz;
			predictionPacket->wx = rcbp->wx;
			predictionPacket->wy = rcbp->wy;
			predictionPacket->wz = rcbp->wz;

			predictionPacket->ax = 0;
			predictionPacket->ay = 0;
			predictionPacket->az = 0;
			// sent the packet to all available clients
			for (client=clients_.begin(); client!=clients_.end(); ++client){
				//sent the packet
				sendPacket((const char*)&cbp, sizeof(CreateBoxPacket), *client);
				//insert a new map.
				predictions_[objectID][*client] = predictionPacket;
			}
			
			
			// leave the critical section
			synchronizeLeave();
			break;

		// a client requested that a sphere would be created
		case ID_REQ_CREATE_SPHERE:

			// make it thread-safe
			synchronizeEnter();

			// get the packet in the correct format
			rcsp = (RequestCreateSpherePacket*)(packet->data);

			// create a sphere in the physics system and retrieve an object id
			objectID = createSphere(rcsp->r, rcsp->x, rcsp->y, rcsp->z, rcsp->qx, rcsp->qy, rcsp->qz, rcsp->qw, rcsp->vx, rcsp->vy, rcsp->vz, rcsp->wx, rcsp->wy, rcsp->wz, rcsp->mass, rcsp->dynamic);
			
			// create a packet to send to the clients
			CreateSpherePacket csp;
			csp.r  = rcsp->r;
			csp.x  = rcsp->x;  csp.y  = rcsp->y;  csp.z  = rcsp->z;
			csp.qx = rcsp->qx; csp.qy = rcsp->qy; csp.qz = rcsp->qz; csp.qw = rcsp->qw;
			csp.vx = rcsp->vx; csp.vy = rcsp->vy; csp.vz = rcsp->vz; 
			csp.dynamic = rcsp->dynamic;
			csp.wx = rcsp->wx; csp.wy = rcsp->wy; csp.wz = rcsp->wz;
			

			csp.typeID   = ID_CREATE_SPHERE;
			csp.objectID = objectID;
			sprintf_s(csp.material, 10, "vw/blue");

			//create a data struct
			
			predictionPacket->lastupdate = getTime();
			predictionPacket->x = rcsp->x;
			predictionPacket->y = rcsp->y;
			predictionPacket->z = rcsp->z;
			predictionPacket->vx = rcsp->vx;
			predictionPacket->vy = rcsp->vy;
			predictionPacket->vz = rcsp->vz;
			predictionPacket->wx = rcsp->wx;
			predictionPacket->wy = rcsp->wy;
			predictionPacket->wz = rcsp->wz;

			predictionPacket->ax = 0;
			predictionPacket->ay = 0;
			predictionPacket->az = 0;
			// sent the packet to all available clients
			for (client=clients_.begin(); client!=clients_.end(); ++client){
				//sent the packet
				sendPacket((const char*)&csp, sizeof(CreateBoxPacket), *client);
				//insert a new map.
				predictions_[objectID][*client] = predictionPacket;
			}
			// leave the critical section
			synchronizeLeave();
			break;
	}
}

/*!
 * \brief Send a packet to the server
 * 
 * \param packet
 * The packet to be send, casted to a const char*
 * 
 * \param size
 * The size in bytes of the message to be send
 * 
 * \param systemAddress
 * The address to which this should be send.
 *
 * \param reliable
 * Indicates if the packet should be send in order or not
 */
void GPServerSystem::sendPacket(const char* packet, const int size, SystemAddress systemAddress, bool reliable) {
	if (reliable) 
		peer_->Send(packet, size, HIGH_PRIORITY, RELIABLE_ORDERED, 0, systemAddress, false); 
	else
		peer_->Send(packet, size, HIGH_PRIORITY, RELIABLE_SEQUENCED, 0, systemAddress, false); 

}




void GPServerSystem::update() {
	// this function should be thread-safe
	GPSYNCHRONIZED;
	std::vector<int>::iterator           object;
	std::vector<SystemAddress>::iterator client;
	std::vector<SystemAddress>::iterator aclient;
	float x,y,z,passedTime,error;
	//float q1,q2,q3,q4;
	PredictionData *prediction = new PredictionData;

	// loop through all the dynamic objects
	for (object = dynamicObjects_.begin(); object != dynamicObjects_.end(); ++object) {
		// create a packet
		UpdateStatePacket usp;

		// get the state from the physics engine 
		physics_->synchronizeEnter();
		physics_->getPosition(*object, usp.x, usp.y, usp.z);
		physics_->getOrientation(*object, usp.qx, usp.qy, usp.qz, usp.qw);

		//get the linearVelocity,and acceleration
		physics_->getLinearVelocity(*object, usp.vx, usp.vy, usp.vz);
		physics_->getAngularVelocity(*object, usp.wx, usp.wy, usp.wz);
		physics_->getAcceleration(*object, usp.ax, usp.ay, usp.az);
		physics_->synchronizeLeave();
		
		// set the object id...
		usp.objectID = *object;
		// ... and the type of the packet
		usp.typeID   = ID_UPDATE_STATE;
		walltime now = getTime();

		// set the packet to all clients that are connected
		for (client=clients_.begin(); client!=clients_.end(); ++client){
			if(predictions_[*object][*client] != NULL){//check if an estimation exist and gives valid results , otherwise send the data
				
				prediction = predictions_[*object][*client];
				passedTime = (float)diffWalltime(prediction->lastupdate, now);
				float normAngular = sqrt( pow(prediction->wx, 2) + pow(prediction->wy, 2) + pow(prediction->wz, 2));
				//calculate the clients estimation
				if(estimationOrder_ == 1){ //Linear extrapolation
					x = prediction->x + prediction->vx * passedTime;
					y = prediction->y + prediction->vy * passedTime;
					z = prediction->z + prediction->vz * passedTime;
					//std::cout <<"S-Obj:"<< *object <<". x " << x << " y "<<  y << " z "<<  z  <<std::endl;
					//linear rotation prediction
					/*q1 = cos(normAngular*(passedTime/2)) * prediction->qx  -  (prediction->wx/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qy -  (prediction->wy/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qz -  (prediction->wz/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qw;
					q2 = cos(normAngular*(passedTime/2)) * prediction->qx  +  (prediction->wx/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qy -  (prediction->wy/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qz +  (prediction->wz/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qw;
					q3 = cos(normAngular*(passedTime/2)) * prediction->qx  +  (prediction->wx/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qy +  (prediction->wy/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qz -  (prediction->wz/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qw;
					q4 = cos(normAngular*(passedTime/2)) * prediction->qx  -  (prediction->wx/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qy +  (prediction->wy/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qz +  (prediction->wz/ normAngular) * sin(normAngular*(passedTime/2))* prediction->qw;
					*/	

				}else if(estimationOrder_ == 2){ //Quadratic extrapolation
					x = (float)(prediction->x + prediction->vx * passedTime + 0.5*prediction->ax*passedTime*passedTime);
					y = (float)(prediction->y + prediction->vy * passedTime + 0.5*prediction->ay*passedTime*passedTime);
					z = (float)(prediction->z + prediction->vz * passedTime + 0.5*prediction->az*passedTime*passedTime);
				}
					
				//float diff1 = abs(usp.qx - q1), diff2 = abs(usp.qy - q2), diff3 = abs(usp.qz - q3),diff4 = abs(usp.qw - q4);
				//float OrienError = sqrt(diff1*diff1 + diff2*diff2 + diff3*diff3+ diff4*diff4);
				//calculate the eucledan distance, and find the error
				float diffx = abs(usp.x - x), diffy = abs(usp.y - y), diffz = abs(usp.z - z);
				error = sqrt(diffx*diffx + diffy*diffy + diffz*diffz);
				//if the error is more thatn the network error send correction packet to all clients
				if(error > atof(settings_["error"].c_str())){
					for (aclient=clients_.begin(); aclient!=clients_.end(); ++aclient){
					sendPacket((const char*)(&usp), sizeof(usp), *aclient, true);
					//std::cout << "Error-Obj:"<<*object<<". SentToClient:"<<aclient->port<<" , " <<usp.x <<" , "<<usp.y <<" , "<< usp.z <<std::endl; 
					}
					
					prediction->lastupdate = getTime();
					prediction->x = usp.x;
					prediction->y = usp.y;
					prediction->z = usp.z;
					prediction->vx = usp.vx;
					prediction->vy = usp.vy;
					prediction->vz = usp.vz;
					prediction->qx = usp.qx;
					prediction->qy = usp.qy;
					prediction->qz = usp.qz;
					prediction->qw = usp.qw;
					prediction->ax = usp.ax;
					prediction->ay = usp.ay;
					prediction->az = usp.az;
					prediction->wx = usp.wx;
					prediction->wy = usp.wy;
					prediction->wz = usp.wz;

					predictions_[*object][*client] = prediction;
					//store all changed values
		
				}
				
			}else{//send packet
				sendPacket((const char*)(&usp), sizeof(usp), *client, true);
			}
		}
	}
}


/*!
 * \brief Initialize a client with the current state of the server
 * 
 * \param systemAddress
 * The address of the client to be initialized
 */
void GPServerSystem::initializeClient(SystemAddress systemAddress) {
	// this function should be thread-safe
	GPSYNCHRONIZED;
	
	// enter the physics critical section
	physics_->synchronizeEnter();

	std::vector<int>::iterator object;
PredictionData *predictionPacket=new PredictionData;
	// create and send a ground plane packet
	CreateGroundPlanePacket cgpp;
	cgpp.typeID = ID_CREATE_GROUNDPLANE;
	sendPacket((const char*)(&cgpp), sizeof(cgpp), systemAddress);

	// loop through all static objects...
	for (object=staticObjects_.begin(); object!=staticObjects_.end(); ++object) {

		// switch to the correct object type for this object
		switch (objectTypes_[*object]) {

			// if the object is a box send a create box packet
			case GP_BOX:
				// create the box packet
				CreateBoxPacket cbp;

				// get the settings for the box
				physics_->getSize(*object, cbp.w, cbp.d, cbp.h);
				physics_->getPosition(*object, cbp.x, cbp.y, cbp.z);
				physics_->getOrientation(*object, cbp.qx, cbp.qy, cbp.qz, cbp.qw);
				cbp.dynamic=false;
				// set the object id
				cbp.objectID = *object;

				// set the packet type
				cbp.typeID = ID_CREATE_BOX;

				// set the material to be used
				sprintf_s(cbp.material, 10, "vw/green");

				// send the packet
				sendPacket((const char*)(&cbp), sizeof(cbp), systemAddress);
				break;

			// if the object is a sphere send a create sphere packet
			case GP_SPHERE:
				// create the sphere packet
				CreateSpherePacket csp;

				// get the settings for the sphere
				physics_->getSize(*object, csp.r);
				physics_->getPosition(*object, csp.x, csp.y, csp.z);
				physics_->getOrientation(*object, csp.qx, csp.qy, csp.qz, csp.qw);
				csp.dynamic=false;
				// set the object id
				csp.objectID = *object;

				// set the packet type
				csp.typeID = ID_CREATE_SPHERE;

				// set the material to be used
				sprintf_s(csp.material, 10, "vw/green");

				// send the packet
				sendPacket((const char*)(&csp), sizeof(csp), systemAddress);
				break;
		}
	}

	// loop through all dynamic objects...
	for (object=dynamicObjects_.begin(); object!=dynamicObjects_.end(); ++object) {
	
		// switch to the correct object type for this object
		switch (objectTypes_[*object]) {

			// if the object is a box send a create box packet
			case GP_BOX:
				// create the box packet
				CreateBoxPacket cbp;

				// get the settings for the box
				physics_->getSize(*object, cbp.w, cbp.d, cbp.h);
				physics_->getPosition(*object, cbp.x, cbp.y, cbp.z);
				physics_->getOrientation(*object, cbp.qx, cbp.qy, cbp.qz, cbp.qw);
				cbp.dynamic=true;
				// set the object id
				cbp.objectID = *object;

				// set the packet type
				cbp.typeID = ID_CREATE_BOX;

				// set the material to be used
				sprintf_s(cbp.material, 10, "vw/red");


				//initialize estimations
				
				predictionPacket->lastupdate = getTime();
				predictionPacket->x = cbp.x;
				predictionPacket->y = cbp.y;
				predictionPacket->z = cbp.z;
				predictionPacket->vx = cbp.vx;
				predictionPacket->vy = cbp.vy;
				predictionPacket->vz = cbp.vz;
				predictionPacket->qx = cbp.qx;
				predictionPacket->qy = cbp.qy;
				predictionPacket->qz = cbp.qz;
				predictionPacket->qw = cbp.qw;
				predictionPacket->wx = cbp.wx;
				predictionPacket->wy = cbp.wy;
     			predictionPacket->wz = cbp.wz;
				predictionPacket->ax = 0;
				predictionPacket->ay = 0;
				predictionPacket->az = 0;
		
				//predictions_[cbp.objectID][systemAddress] = predictionPacket;
				// send the packet
				sendPacket((const char*)(&cbp), sizeof(cbp), systemAddress);
				break;

			// if the object is a sphere send a create sphere packet
			case GP_SPHERE:
				// create the sphere packet
				CreateSpherePacket csp;

				// get the settings for the sphere
				physics_->getSize(*object, csp.r);
				physics_->getPosition(*object, csp.x, csp.y, csp.z);
				physics_->getOrientation(*object, csp.qx, csp.qy, csp.qz, csp.qw);
				//csp.dynamic=true;
				// set the object id
				csp.objectID = *object;

				// set the packet type
				csp.typeID = ID_CREATE_SPHERE;

				// set the material to be used
				sprintf_s(csp.material, 10, "vw/blue");
				//initialize estimations
				
				predictionPacket->lastupdate = getTime();
				predictionPacket->x = csp.x;
				predictionPacket->y = csp.y;
				predictionPacket->z = csp.z;
				predictionPacket->vx = csp.vx;
				predictionPacket->vy = csp.vy;
				predictionPacket->vz = csp.vz;
				predictionPacket->qx = csp.qx;
				predictionPacket->qy = csp.qy;
				predictionPacket->qz = csp.qz;
				predictionPacket->qw = csp.qw;
				predictionPacket->wx = csp.wx;
				predictionPacket->wy = csp.wy;
     			predictionPacket->wz = csp.wz;
				predictionPacket->ax = 0;
				predictionPacket->ay = 0;
				predictionPacket->az = 0;


				// send the packet
				sendPacket((const char*)(&csp), sizeof(csp), systemAddress);
				break;
		}
	}

	// leave the physics critical section
	physics_->synchronizeLeave();

}

/*!
 * \brief Creates the ground plane in the physics system
 * 
 * \returns 0
 */
int GPServerSystem::createGroundPlane() {
	// create the ground plane in the physics system
	physics_->createGroundPlane();
	return 0;
}

/*!
 * \brief This function creates a box.
 * 
 * \param w The width of the box
 * \param h The height of the box
 * \param d The depth of the box
 * \param x The x position of the box
 * \param y The y position of the box
 * \param z The z position of the box
 * \param qx The x component of the quaternion describing the orientation of the box
 * \param qy The y component of the quaternion describing the orientation of the box
 * \param qz The z component of the quaternion describing the orientation of the box
 * \param qw The w component of the quaternion describing the orientation of the box
 * \param vx The x component of the linear velocity
 * \param vy The y component of the linear velocity
 * \param vz The z component of the linear velocity
 * \param wx The x component of the angular velocity
 * \param wy The y component of the angular velocity
 * \param wz The z component of the angular velocity
 * \param mass The mass of the object
 * \param dynamic This parameter indicates whether the object is dynamic or static
 *
 * \returns The object id of the newly created object
 *
 * This function creates a box in the physics engine and stores the create object id in the correct list.
 */
int GPServerSystem::createBox(float w, float h, float d,
							  float x, float y, float z,
							  float qx, float qy, float qz, float qw,
							  float vx, float vy, float vz,
							  float wx, float wy, float wz, 
							  float mass, float dynamic) 
{
	// create the box in the physics system (this function is thread-safe)
	int objectID = physics_->createBox(w, h, d, x, y, z, qx, qy, qz, qw, vx, vy, vz, wx, wy, wz, mass, dynamic);

	// store the object id in the correct list
	if (dynamic) dynamicObjects_.push_back(objectID);
	else         staticObjects_.push_back(objectID);

	// store the object type
	objectTypes_[objectID] = GP_BOX;

	// return the object id
	return objectID;
}

/*!
 * \brief This function creates a sphere.
 * 
 * \param r The radius of the sphere
 * \param x The x position of the sphere
 * \param y The y position of the sphere
 * \param z The z position of the sphere
 * \param qx The x component of the quaternion describing the orientation of the sphere
 * \param qy The y component of the quaternion describing the orientation of the sphere
 * \param qz The z component of the quaternion describing the orientation of the sphere
 * \param qw The w component of the quaternion describing the orientation of the sphere
 * \param vx The x component of the linear velocity
 * \param vy The y component of the linear velocity
 * \param vz The z component of the linear velocity
 * \param wx The x component of the angular velocity
 * \param wy The y component of the angular velocity
 * \param wz The z component of the angular velocity
 * \param mass The mass of the object
 * \param dynamic This parameter indicates whether the object is dynamic or static
 *
 * \returns The object id of the newly created object
 *
 * This function creates a sphere in the physics engine and stores the create object id in the correct list.
 */
int GPServerSystem::createSphere(float r,
				 float x, float y, float z,
				 float qx, float qy, float qz, float qw,
				 float vx, float vy, float vz,
				 float wx, float wy, float wz, 
				 float mass, float dynamic)
{
	// create the box in the physics system (this function is thread-safe)
	int objectID = physics_->createSphere(r, x, y, z, qx, qy, qz, qw, vx, vy, vz, wx, wy, wz, mass, dynamic);

	// store the object id in the correct list
	if (dynamic) dynamicObjects_.push_back(objectID);
	else         staticObjects_.push_back(objectID);

	// store the object type
	objectTypes_[objectID] = GP_SPHERE;

	// return the object id
	return objectID;
}

/*!
 * \brief This function removes all spaces at the beginning and the end of a string
 * \param value The string that should be stripped from all leading and final spaces.
 */
extern void trimspaces(std::string& value);

/*!
 * \brief Reads a settings file.
 * 
 * \param filename The name of the file containing the settings
 * 
 * This function reads a file specified by the filename argument. The
 * file is a \<property\>=\<value\> file. If the first character of a line is
 * a \#, the line is considered a comment line.
 */
void GPServerSystem::loadSettings(const std::string& filename) {
	// first clear the existing settings
	settings_.clear();
	// create an input stream
	std::ifstream settingsfile(filename.c_str());
	// the string variable containing the next line
	std::string line;
	// while the file has not reached its end...
	while (std::getline(settingsfile, line)) {
		// ... check if the line is not a comment line ...
		if (line.c_str()[0]!='#') {
			// ... and find the '=' sign.
			size_t pos = line.find_first_of("=");
			std::string param, value;
			// get the first part (property/param) of the string
			param = line.substr(0,pos);  trimspaces(param);
			// get the last part (value) of the string
			value = line.substr(pos+1);  trimspaces(value);
			// store the param/value pair in the settings_ variable
			settings_[param]=value;
		}
	}
	// finally, close the input stream
	settingsfile.close();
}
